#include <convert_manager.h>
#include <video_manager.h>
#include <config.h>
#include <stdint.h>
#include <stdlib.h>
#include "color.h"

// 定义RGB565的宏
#define RGB565(r, g, b) (((r >> 3) << 11) | ((g >> 2) << 5) | (b >> 3))

int Yuv2RgbisSupport(int iPixelFormatIn,int iPixelFormatOut)
{
    if(iPixelFormatIn != V4L2_PIX_FMT_YUYV)
        return -1;

    if((iPixelFormatOut != V4L2_PIX_FMT_RGB565) && (iPixelFormatOut != V4L2_PIX_FMT_RGB32))
        return -1;
    
    return 0;
}



/* 
//更直白的写法
void yuyv_to_rgb565(uint8_t *yuyv, uint16_t *rgb565, int width, int height) {
    int i, j;
    int y0, y1, u, v;
    int r, g, b;

    for (i = 0; i < height; i++) {
        for (j = 0; j < width - 1; j += 2) {
            // 读取YUYV数据
            y0 = yuyv[(i * width + j) * 2];
            u = yuyv[(i * width + j) * 2 + 1];
            y1 = yuyv[(i * width + j) * 2 + 2];
            v = yuyv[(i * width + j) * 2 + 3];

            // 转换第一个像素 (Y0, U, V) -> RGB
            r = y0 + 1.402 * (v - 128);
            g = y0 - 0.344 * (u - 128) - 0.714 * (v - 128);
            b = y0 + 1.772 * (u - 128);

            // 限制RGB值在0-255之间
            r = r < 0 ? 0 : (r > 255 ? 255 : r);
            g = g < 0 ? 0 : (g > 255 ? 255 : g);
            b = b < 0 ? 0 : (b > 255 ? 255 : b);

            // 转换为RGB565并存储
            rgb565[i * width + j] = RGB565(r, g, b);

            // 转换第二个像素 (Y1, U, V) -> RGB
            r = y1 + 1.402 * (v - 128);
            g = y1 - 0.344 * (u - 128) - 0.714 * (v - 128);
            b = y1 + 1.772 * (u - 128);

            // 限制RGB值在0-255之间
            r = r < 0 ? 0 : (r > 255 ? 255 : r);
            g = g < 0 ? 0 : (g > 255 ? 255 : g);
            b = b < 0 ? 0 : (b > 255 ? 255 : b);

            // 转换为RGB565并存储
            rgb565[i * width + j + 1] = RGB565(r, g, b);
        }

        // 如果宽度是奇数，处理最后一个像素
        if (width % 2 == 1) {
            // 读取YUYV数据
            y0 = yuyv[(i * width + width - 1) * 2];
            u = yuyv[(i * width + width - 2) * 2 + 1]; // 使用前一个像素的U分量
            v = yuyv[(i * width + width - 2) * 2 + 3]; // 使用前一个像素的V分量

            // 转换最后一个像素 (Y0, U, V) -> RGB
            r = y0 + 1.402 * (v - 128);
            g = y0 - 0.344 * (u - 128) - 0.714 * (v - 128);
            b = y0 + 1.772 * (u - 128);

            // 限制RGB值在0-255之间
            r = r < 0 ? 0 : (r > 255 ? 255 : r);
            g = g < 0 ? 0 : (g > 255 ? 255 : g);
            b = b < 0 ? 0 : (b > 255 ? 255 : b);

            // 转换为RGB565并存储
            rgb565[i * width + width - 1] = RGB565(r, g, b);
        }
    }
}
*/


// YUYV转RGB565函数
void yuyv_to_rgb565(uint8_t *yuyv, uint8_t *rgb565, int width, int height) 
{
    int y0, y1, u, v;
    int r, g, b;
    uint16_t color;

    //一个YUV2保存了两个像素点的数据，故实际的数组大小为分辨率的一半
    //这里我再额外补充介绍一下
    //对于YUV2格式的数据，一个YUV2含两个像素点的数据，而每个YUV2使用4个字节表示，即Y0 U Y1 V
    //所以对于一个分辨率为width*height的YUV2图像，实际的数组大小为width*height * 2
    //可以简单去想，假设只有两个像素，那么会使用一个YUV2，即4个字节，所以实际数组大小为 1 * 2 * 2 = 4
    int size = width * height / 2;
    for(int i = size; i > 0; i--)
    {
        y0 = yuyv[0];
        u = yuyv[1];
        y1 = yuyv[2];
        v = yuyv[3];
        yuyv += 4;

        r = R_FROMYV(y0, v);
        g = G_FROMYUV(y0, u, v);
        b = B_FROMYU(y0, u);
        color = RGB565(r, g, b);
        *rgb565++ = color & 0xff;
        *rgb565++ = (color >> 8) & 0xff;

        r = R_FROMYV(y1, v);
        g = G_FROMYUV(y1, u, v);
        b = B_FROMYU(y1, u);
        color = RGB565(r, g, b);
        *rgb565++ = color & 0xff;
        *rgb565++ = (color >> 8) & 0xff;
    } 
}

// YUYV转RGB32函数
void yuyv_to_rgb32(uint8_t *yuyv, uint8_t *rgb32, int width, int height) 
{
    int y0, y1, u, v;
    int r, g, b;

    int size = width * height / 2;
    for(int i = size; i > 0; i--)
    {
        y0 = yuyv[0];
        u = yuyv[1];
        y1 = yuyv[2];
        v = yuyv[3];
        yuyv += 4;

        r = R_FROMYV(y0, v);
        g = G_FROMYUV(y0, u, v);
        b = B_FROMYU(y0, u);
        *rgb32++ = r;
        *rgb32++ = g;
        *rgb32++ = b;
        *rgb32++ = 0;

        r = R_FROMYV(y1, v);
        g = G_FROMYUV(y1, u, v);
        b = B_FROMYU(y1, u);
        *rgb32++ = r;
        *rgb32++ = g;
        *rgb32++ = b;
        *rgb32++ = 0;
    } 
}

int Yuv2RgbConvert(PT_VideoBuf iVideoBufIn,PT_VideoBuf iVideoBufOut)
{
    PT_PixelDatas ptPixelDatasIn = &iVideoBufIn->tPixelDatas;
    PT_PixelDatas ptPixelDatasOut = &iVideoBufOut->tPixelDatas;
    
    int width = ptPixelDatasIn->iWidth;
    int height = ptPixelDatasIn->iHeight;

    if(iVideoBufOut->iPixelFormat == V4L2_PIX_FMT_RGB565)
    {
        ptPixelDatasOut->iBpp = 16;
        ptPixelDatasOut->iLineBytes = width * ptPixelDatasOut->iBpp / 8;
        ptPixelDatasOut->iTotalBytes = ptPixelDatasOut->iLineBytes * height;

        if(ptPixelDatasOut->aucPixelDatas == NULL)
        {
            ptPixelDatasOut->aucPixelDatas = (unsigned char *)malloc(ptPixelDatasOut->iTotalBytes);
        }

        yuyv_to_rgb565(ptPixelDatasIn->aucPixelDatas, ptPixelDatasOut->aucPixelDatas, width, height);

        return 0;
    }
    else if(iVideoBufOut->iPixelFormat == V4L2_PIX_FMT_RGB32)
    {
        ptPixelDatasOut->iBpp = 32;
        ptPixelDatasOut->iLineBytes = width * ptPixelDatasOut->iBpp / 8;
        ptPixelDatasOut->iTotalBytes = ptPixelDatasOut->iLineBytes * height;

        if(ptPixelDatasOut->aucPixelDatas == NULL)
        {
            ptPixelDatasOut->aucPixelDatas = (unsigned char *)malloc(ptPixelDatasOut->iTotalBytes);
        }
        
        yuyv_to_rgb32(ptPixelDatasIn->aucPixelDatas, ptPixelDatasOut->aucPixelDatas, width, height);

         return 0;
    }
    
    return -1;
}

int Yuv2RgbConvertExit(PT_VideoBuf iVideoBufOut)
{
    if(iVideoBufOut->tPixelDatas.aucPixelDatas)
    {
        free(iVideoBufOut->tPixelDatas.aucPixelDatas);
        iVideoBufOut->tPixelDatas.aucPixelDatas = NULL;
    }
    
    return 0;
}


static T_VideoConvert g_tYuv2RgbConvert = {
    .name = "yuv2rgb",
    .isSupport = Yuv2RgbisSupport,
    .Convert = Yuv2RgbConvert,
    .ConvertExit = Yuv2RgbConvertExit,
};

/*注册*/

int Yuv2RgbInit(void)
{
    initLut();
    return RegisterVideoConvert(&g_tYuv2RgbConvert);;
}